#include "utils.h"
#include <QtCore/qmath.h>

const double Utils::Radius = 637100900;

qreal Utils::shortestDistance(const Point & p, const Point & s1, const Point & s2, const Point & s3)
{
    qreal x = p.x();
    qreal y = p.y();
    qreal z = p.z();
    qreal x1 = s1.x();
    qreal y1 = s1.y();
    qreal z1 = s1.z();
    qreal x2 = s2.x();
    qreal y2 = s2.y();
    qreal z2 = s2.z();
    qreal x3 = s3.x();
    qreal y3 = s3.y();
    qreal z3 = s3.z();
    qreal A = y1 * (z2 - z3) + y2 * (z3 - z1) + y3 * (z1 - z2);
    qreal B = z1 * (x2 - x3) + z2 * (x3 - x1) + z3 * (x1 - x2);
    qreal C = x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2);
    qreal D = - x1 * (y2 * z3 - y3 * z2) - x2 * (y3 * z1 - y1 * z3) - x3 * (y1 * z2 - y2 * z1);
    return (A * x + B * y + C * z + D) / qSqrt(A * A + B * B + C * C);
}

qreal Utils::shortestDistance(const Point & p1, const Point & p2)
{
    return qSqrt(qPow(p1.x() - p2.x(), 2.0) + qPow(p1.y() - p2.y(), 2.0) + qPow(p1.z() - p2.z(), 2.0));
}

qreal Utils::angleDistance(const Point & p1, const Point & p2)
{
    Point p = p1;
    p.setAltitude(p2.getAltitude());
    return Utils::toDegrees(2.0 * qAsin(shortestDistance(p, p2) / (2.0 * Utils::Radius)));
}

inline double Utils::scalarProduct(int x1, int y1, int z1, int x2, int y2, int z2)
{
    return x1 * x2 + y1 * y2 + z1 * z2;
}

bool Utils::intersects(const Segment & segment, const Point2d & center, double angleRadius)
{
    Point p11(segment.bottomLeft().getLatitude() + segment.size().height(), segment.bottomLeft().getLongitude());
    Point p12(segment.bottomLeft().getLatitude() + segment.size().height(), segment.bottomLeft().getLongitude() + segment.size().width());
    Point p21(segment.bottomLeft().getLatitude(), segment.bottomLeft().getLongitude());
    Point p22(segment.bottomLeft().getLatitude(), segment.bottomLeft().getLongitude() + segment.size().width());
    Point c(segment.bottomLeft().getLatitude() + segment.size().height() / 2.0, segment.bottomLeft().getLongitude() + segment.size().width() / 2.0);
    Point c0(center);
    p11.normalize();
    p12.normalize();
    p21.normalize();
    p22.normalize();
    c.normalize();
    c0.normalize();
    double r = 2.0 * Radius * qSin(toRadians(angleRadius / 2.0));
    return Utils::shortestDistance(c0, c) < r || Utils::shortestDistance(c0, p11) < r || Utils::shortestDistance(c0, p12) < r
            || Utils::shortestDistance(c0, p21) < r || Utils::shortestDistance(c0, p22) < r;
/*
    if(center.getLatitude() > p11.getLatitude())
    {
        // TODO fix this (what if center is closer to p12, but center.getLongitude() == -179 and p12.getLongitude() == 179)
        if(center.getLongitude() < p11.getLongitude())
            return r > shortestDistance(c, p11);
        else if(center.getLongitude() > p12.getLongitude())
            return r > shortestDistance(c, p12);
        else
            return intersects(p11, p12, c, r);
    } else if(center.getLatitude() < p21.getLatitude())
    {
        if(center.getLongitude() < p21.getLongitude())
            return r > shortestDistance(c, p21);
        else if(center.getLongitude() > p22.getLongitude())
            return r > shortestDistance(c, p22);
        else
            return intersects(p21, p22, c, r);
    } else
    {
        if(center.getLongitude() < p11.getLongitude())
            return intersects(p11, p21, c, r);
        else if(center.getLongitude() > p12.getLongitude())
            return intersects(p12, p22, c, r);
        else
            return true;
    }
*/
}

bool Utils::intersects(const Point & p1, const Point & p2, const Point & center, double radius)
{
    double a = shortestDistance(center, p1);
    double b = shortestDistance(center, p2);
    double c = shortestDistance(p1, p2);
    int x1 = p1.x();
    int y1 = p1.y();
    int z1 = p1.z();
    int x2 = p2.x();
    int y2 = p2.y();
    int z2 = p2.z();
    int xC = center.x();
    int yC = center.y();
    int zC = center.z();
    if(scalarProduct(xC - x1, yC - y1, zC - z1, x2 - x1, y2 - y1, z2 - z1) / (a * c) <= 0)
        return radius > a;
    if(scalarProduct(xC - x2, yC - y2, zC - z2, x1 - x2, y1 - y2, z1 - z2) / (b * c) <= 0)
        return radius > b;
    double cosAB = scalarProduct(x1 - xC, y1 - yC, z1 - zC, x2 - xC, y2 - yC, z2 - zC) / (a * b);
    return radius > qSqrt(1.0 - cosAB * cosAB) * a * b / c;
}

double Utils::toRadians(double degrees)
{
    return M_PI * degrees / 180.0;
}

double Utils::toDegrees(double radians)
{
    return 180.00 * radians / M_PI;
}
